function vkn = Speedupdate(cbn_k_1,v_k_2n,v_k_1n,position_k_2,position_k_1,data_k_1,data_k,correct)
%已知量：前两个时刻的速度、纬度、高程
%已知量：前一个时刻的姿态矩阵
%已知量：前一个时刻和本时刻之间的速度、角度变化量
%实现速度更新
time=data_k(1);
t11=446285.019;t12=446604.720; 
t21=446851.045;t22=446921.847;
t31=447157.421;t32=447218.768;
t41=447484.043;t42=447547.455;
t51=447801.940;
if(correct=="yes")

    if (time>= t11&&time<= t12)||(time>= t21&&time<= t22)||(time>= t31&&time<= t32)||(time>= t41&&time<= t42)||(time>= t51)%在零速修正的时间范围内
        vkn = [0;0;0];
    else
        %变量初始化
        phi_k_1    =position_k_1(1);
        h_k_1n     =position_k_1(3);
        phi_k_2    =position_k_2(1);
        h_k_2n     =position_k_2(3);
        dv_k_1     =[data_k_1(5); data_k_1(6); data_k_1(7)];
        dv_k       =[data_k(5); data_k(6); data_k(7)];
        dTheta_k_1 =[data_k_1(2); data_k_1(3); data_k_1(4)];
        dTheta_k   =[data_k(2); data_k(3); data_k(4)];

        %外推
        v_k_1_2n  = 1.5*v_k_1n - 0.5*v_k_2n;
        phi_k_1_2 = 1.5*phi_k_1- 0.5*phi_k_2;
        h_k_1_2n  = 1.5*h_k_1n - 0.5*h_k_2n;

        %重力、哥氏积分项
        wien      = wienGet(phi_k_1_2);
        R         = rmnGet(phi_k_1_2);
        wenn      = wennGet(v_k_1_2n,R,h_k_1_2n,phi_k_1_2);
        gpn       = [0; 0; gpnGet(phi_k_1_2,h_k_1_2n);];
        detat     = 0.005;

        delta_v_gcorkn = (gpn-cross((2*wien+wenn),v_k_1_2n))*detat;

        %比力积分项
        detat          =0.005;
        delta_v_fkbk_1 = dv_k+0.5*cross(dTheta_k,dv_k)+(1/12.0)*(cross(dTheta_k_1,dv_k)+cross(dv_k_1,dTheta_k));
        Zeta_nk_1nk    = (wien+wenn)*detat;
        skew_Zeta      = skewMatrix(Zeta_nk_1nk);
        I=eye(3);
        delta_v_fkn    = (I-0.5*skew_Zeta)*cbn_k_1*delta_v_fkbk_1;
    
        %更新
        vkn = v_k_1n + delta_v_fkn + delta_v_gcorkn;
    end
elseif(correct=="no")
    %变量初始化
        phi_k_1    =position_k_1(1);
        h_k_1n     =position_k_1(3);
        phi_k_2    =position_k_2(1);
        h_k_2n     =position_k_2(3);
        dv_k_1     =[data_k_1(5); data_k_1(6); data_k_1(7)];
        dv_k       =[data_k(5); data_k(6); data_k(7)];
        dTheta_k_1 =[data_k_1(2); data_k_1(3); data_k_1(4)];
        dTheta_k   =[data_k(2); data_k(3); data_k(4)];

        %外推
        v_k_1_2n  = 1.5*v_k_1n - 0.5*v_k_2n;
        phi_k_1_2 = 1.5*phi_k_1- 0.5*phi_k_2;
        h_k_1_2n  = 1.5*h_k_1n - 0.5*h_k_2n;

        %重力、哥氏积分项
        wien      = wienGet(phi_k_1_2);
        R         = rmnGet(phi_k_1_2);
        wenn      = wennGet(v_k_1_2n,R,h_k_1_2n,phi_k_1_2);
        gpn       = [0; 0; gpnGet(phi_k_1_2,h_k_1_2n);];
        detat     = 0.005;

        delta_v_gcorkn = (gpn-cross((2*wien+wenn),v_k_1_2n))*detat;

        %比力积分项
        detat          =0.005;
        delta_v_fkbk_1 = dv_k+0.5*cross(dTheta_k,dv_k)+(1/12.0)*(cross(dTheta_k_1,dv_k)+cross(dv_k_1,dTheta_k));
        Zeta_nk_1nk    = (wien+wenn)*detat;
        skew_Zeta      = skewMatrix(Zeta_nk_1nk);
        I=eye(3);
        delta_v_fkn    = (I-0.5*skew_Zeta)*cbn_k_1*delta_v_fkbk_1;
    
        %更新
        vkn = v_k_1n + delta_v_fkn + delta_v_gcorkn;
end

end
%%
%辅助函数

function R = rmnGet(phi)
%已知量：纬度
%获得某个纬度处的子午圈半径：RM、卯酉圈半径：RN;
    a    = 6378137.0;
    e    = 0.08181919104;
    RM=(a * (1 - e^2)) / (1 - e^2 * sin(phi)^2)^(3/2);
    RN=a / sqrt(1 - e^2 * sin(phi)^2);
    R=[RM; RN];
end

function wien = wienGet(phi)
%已知量：纬度
%获得地球自转角速度在n系下的投影
    we    = 7.292115E-5;
    wien  = [we*cos(phi); 0; -we*sin(phi)];
end

function wenn = wennGet(v,R,h,phi)
%已知量：纬度、子午圈目卯酉圈半径、高程
%获得载体相对地球角速度在n系下的投影
    RM=R(1);
    RN=R(2);
    vN=v(1);
    vE=v(2);
    wenn=[vE/(RN+h); -vN/(RM+h); -vE*tan(phi)/(RN+h);];
end
%%
function A = skewMatrix(v)
%得到向量对应的反对称矩阵
%输入v是一个3x1的向量
    A = [   0  , -v(3),  v(2);
           v(3),   0  , -v(1);
          -v(2),  v(1) ,  0 ];
end
%%
function gpn = gpnGet(phi,h)
%已知量：纬度、高程
%获得某纬度高度处的重力
    g0     = 9.7803267715*(1+0.0052790414*sin(phi)^2+0.0000232718*sin(phi)^4);
    gphih  = g0-(3.087691089E-6-4.397731E-9*sin(phi)^2)*h+0.721E-12*h*h;
    gpn    = gphih;

end